#ifndef FLANN_WRAPPER_H
#define FLANN_WRAPPER_H

#include <flann/flann.hpp>
#include <map>
#include <vector>
#include <iterator>
#include "Point.h"

class Flann_wrapper
{
private:
  typedef flann::Matrix<double>				Point_flann;
  typedef flann::Index<flann::L2<double> >	Index_flann;

  typedef std::map<Point, unsigned int, Less_than_point_lex>	Point_indx_map;
private:
  Index_flann*		index_ptr;
  Point_indx_map	point_indx_map;
public:
  //constructor
  Flann_wrapper() : index_ptr(NULL)
  {}
  ~Flann_wrapper() 
  {
	  if (index_ptr != NULL)
		  delete index_ptr;
  }
  void insert(const Point& p)
  {
	  if (index_ptr == NULL)
	  {
		  //first insertion
		  //index_ptr = new Index_flann(flann::KDTreeIndexParams(4));
		  index_ptr = new Index_flann(flann::KDTreeIndexParams(1));
		  //index_ptr = new Index_flann(flann::LshIndexParams(12,20,0));
		  //index_ptr = new Index_flann(flann::LshIndexParams());
		  //index_ptr = new Index_flann(flann::AutotunedIndexParams());
		  index_ptr->buildIndex(to_flann_point(p));
	  }
	  else
	  {
		  index_ptr->addPoints(to_flann_point(p));
	  }

	  point_indx_map[p] = p.index();
	  return;
  }
  Point nearest_neighbor(const Point& q) 
  {
	  std::vector<Point> res;
	  k_nearest_neighbors(	q, 1, 
							std::back_inserter(res));
	  return res.front();
  }
  template <typename OutputIterator>
  void k_nearest_neighbors(	const Point& q, int k, 
							OutputIterator& oi) const
  {
	  flann::Matrix<int>	query_index(new int[k], //k ids for the returned k nearest neighbors
										1,			//we alwayse use 1 query at a time
										k);			//number of nearest neighbors 
	  
	  flann::Matrix<double>		query_dists(new double[k],
										1, 
										k); 
    
	  flann::SearchParams search_param;
	  //search_param.cores = 1;
	  index_ptr->knnSearch(	to_flann_point(q),			//query point
							query_index,				//Matrix that will contain the indices of the K-nearest neighbors found
							query_dists,				//Matrix that will contain the distances to the K-nearest neighbors found
							k,							//1 nearest neighbor
							flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));	//search all leafs
							//flann::SearchParams());
	  	  
	  for (int i(0); i <k; ++i)
	  {
		  unsigned int index = query_index[0][i];
		  double* nearest_neighbor = index_ptr->getPoint(index);
		  *oi++ = to_point(nearest_neighbor, q.dimension());
	  }
	  delete[] query_index.ptr();
	  delete[] query_dists.ptr();
    return ;
  }

  template <typename OutputIterator>
  void r_nearest_neighbors(	const Point& q, double r, //r is squared radius 
							OutputIterator& oi) const
  {
	  //int max = index_ptr->size() / 10;
	  int max = index_ptr->size() ;
	  flann::Matrix<int>	query_index(new int[max], //ids for the returned nearest neighbors
										1,								//we alwayse use 1 query at a time
										max);								//max number of nearest neighbors 
	  
	  flann::Matrix<double>		query_dists(new double[max],
										1, 
										max); 
    

	  int cnt = index_ptr->radiusSearch(to_flann_point(q),			//query point
										query_index,				//Matrix that will contain the indices of the K-nearest neighbors found
										query_dists,				//Matrix that will contain the distances to the K-nearest neighbors found
										static_cast<float> (r),							//radius
										flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));	//search all leafs
										//flann::SearchParams());
	  	  
	  for (int i(0); i <cnt; ++i)
	  {
		  unsigned int index = query_index[0][i];
		  double* nearest_neighbor = index_ptr->getPoint(index);
		  *oi++ = to_point(nearest_neighbor, q.dimension());
	  }
	  delete[] query_index.ptr();
	  delete[] query_dists.ptr();
    return ;
  }

private:
  Point_flann to_flann_point(const Point& p) const
  {
	  int dim = p.dimension();
	  double* raw_point =  new double[dim];
	  
	  for (int i(0); i <dim; ++i)
		raw_point[i] = p[i];
	  
	  return Point_flann(	raw_point,	//coordinates
							1,			//we habe one point
							dim);		//dimension
  }
  Point to_point(const Point_flann& p, int dim) const
  {
	  double* raw_point = p[1]; 

	  return to_point(raw_point, dim);
  }
  Point to_point(const double* raw_point, int dim) const
  {  
	  Point p(raw_point[0], raw_point[1], raw_point[2]);
	  p.set_index(point_indx_map.find(p)->second);
	  return p;
  }
};  //Flann_wrapper

#endif //FLANN_WRAPPER_H